/*
 * nav_recip_common_test.cpp
 * nav_recip_common
 *
 *
 * Copyright (c) 2008-2012, University of North Carolina at Chapel Hill
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of the University of North Carolina at Chapel Hill nor
 *     the names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 *     Dept. of Computer Science
 *     201 S. Columbia St.
 *     Frederick P. Brooks, Jr. Computer Science Bldg.
 *     Chapel Hill, N.C. 27599-3175
 *     United States of America
 *
 *     http://gamma.cs.unc.edu/ROS/
 */

#include <gtest/gtest.h>
#include <nav_recip_common/nav_recip_common.h>

namespace nav_recip
{
// Test fixture for Robot and robot.h.
class RobotTest : public ::testing::Test { };

// Test fixture for scalar.h.
class ScalarTest : public ::testing::Test { };

// Test fixture for Vector2 and vector2.h.
class Vector2Test : public ::testing::Test { };

// Test constructors in Robot.
TEST_F(RobotTest, Constructor)
{
  // Default constructor.
  const Robot robot1;
  
  EXPECT_FLOAT_EQ(0.0f, robot1.radius_);
  EXPECT_FLOAT_EQ(0.0f, robot1.position_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.position_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot1.velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.velocity_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot1.preferred_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.preferred_velocity_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot1.max_speed_);
  EXPECT_FLOAT_EQ(0.0f, robot1.new_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.new_velocity_.getY());
  EXPECT_TRUE(robot1.neighbors_.empty());
  
  // Constructor given bounding radius, current position, and current linear
  // velocity.
  const Robot robot2(1.0f, Vector2(2.0f, 3.0f), Vector2(4.0f, 5.0f));
  
  EXPECT_FLOAT_EQ(1.0f, robot2.radius_);
  EXPECT_FLOAT_EQ(2.0f, robot2.position_.getX());
  EXPECT_FLOAT_EQ(3.0f, robot2.position_.getY());
  EXPECT_FLOAT_EQ(4.0f, robot2.velocity_.getX());
  EXPECT_FLOAT_EQ(5.0f, robot2.velocity_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot2.preferred_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot2.preferred_velocity_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot2.max_speed_);
  EXPECT_FLOAT_EQ(0.0f, robot2.new_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot2.new_velocity_.getY());
  EXPECT_TRUE(robot2.neighbors_.empty());
  
  // Constructor given bounding radius, current position, current linear
  // velocity, preferred linear velocity, and maximum linear speed.
  const Robot robot3(6.0f, Vector2(7.0f, 8.0f), Vector2(9.0f, 10.0f), Vector2(11.0f, 12.0f), 13.0f);
  
  EXPECT_FLOAT_EQ(6.0f, robot3.radius_);
  EXPECT_FLOAT_EQ(7.0f, robot3.position_.getX());
  EXPECT_FLOAT_EQ(8.0f, robot3.position_.getY());
  EXPECT_FLOAT_EQ(9.0f, robot3.velocity_.getX());
  EXPECT_FLOAT_EQ(10.0f, robot3.velocity_.getY());
  EXPECT_FLOAT_EQ(11.0f, robot3.preferred_velocity_.getX());
  EXPECT_FLOAT_EQ(12.0f, robot3.preferred_velocity_.getY());
  EXPECT_FLOAT_EQ(13.0f, robot3.max_speed_);
  EXPECT_FLOAT_EQ(0.0f, robot3.new_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot3.new_velocity_.getY());
  EXPECT_TRUE(robot2.neighbors_.empty());
}

// Test getters in Robot.
TEST_F(RobotTest, Get)
{
  Robot robot(2.0f, Vector2(4.0f, 6.0f), Vector2(8.0f, 10.0f), Vector2(12.0f, 14.0f), 16.0f);
  
  robot.new_velocity_ = Vector2(18.0f, 20.0f);
  
  Robot neighbor(22.0f, Vector2(24.0f, 26.0f), Vector2(28.0f, 30.0f));
  robot.neighbors_.push_back(&neighbor);
  
  const float radius = robot.getRadius();
  const Vector2& position = robot.getPosition();
  const Vector2& velocity = robot.getVelocity();
  const Vector2& preferred_velocity = robot.getPreferredVelocity();
  const float max_speed = robot.getMaxSpeed();
  const Vector2& new_velocity = robot.getNewVelocity();
  const ConstRobotList& neighbors = robot.getNeighbors();
  
  EXPECT_FLOAT_EQ(2.0f, radius);
  EXPECT_FLOAT_EQ(4.0f, position.getX());
  EXPECT_FLOAT_EQ(6.0f, position.getY());
  EXPECT_FLOAT_EQ(8.0f, velocity.getX());
  EXPECT_FLOAT_EQ(10.0f, velocity.getY());
  EXPECT_FLOAT_EQ(12.0f, preferred_velocity.getX());
  EXPECT_FLOAT_EQ(14.0f, preferred_velocity.getY());
  EXPECT_FLOAT_EQ(16.0f, max_speed);
  EXPECT_FLOAT_EQ(18.0f, new_velocity.getX());
  EXPECT_FLOAT_EQ(20.0f, new_velocity.getY());
  EXPECT_EQ(1, neighbors.size());
  EXPECT_FLOAT_EQ(22.0f, neighbors[0]->radius_);
}

// Test setters in Robot.
TEST_F(RobotTest, Set)
{
  Robot robot;
  
  robot.setRadius(3.0f);
  robot.setPosition(Vector2(6.0, 9.0f));
  robot.setVelocity(Vector2(12.0, 15.0f));
  robot.setPreferredVelocity(Vector2(18.0, 21.0f));
  robot.setMaxSpeed(24.0f);
  
  ConstRobotList neighbors;
  Robot neighbor(27.0f, Vector2(30.0f, 33.0f), Vector2(42.0f, 45.0f));
  neighbors.push_back(&neighbor);
  robot.setNeighbors(neighbors);
  
  EXPECT_FLOAT_EQ(3.0f, robot.getRadius());
  EXPECT_FLOAT_EQ(6.0f, robot.getPosition().getX());
  EXPECT_FLOAT_EQ(9.0f, robot.getPosition().getY());
  EXPECT_FLOAT_EQ(12.0f, robot.getVelocity().getX());
  EXPECT_FLOAT_EQ(15.0f, robot.getVelocity().getY());
  EXPECT_FLOAT_EQ(18.0f, robot.getPreferredVelocity().getX());
  EXPECT_FLOAT_EQ(21.0f, robot.getPreferredVelocity().getY());
  EXPECT_FLOAT_EQ(24.0f, robot.getMaxSpeed());
  EXPECT_FLOAT_EQ(0.0f, robot.getNewVelocity().getX());
  EXPECT_FLOAT_EQ(0.0f, robot.getNewVelocity().getY());
  EXPECT_EQ(1, robot.getNeighbors().size());
  EXPECT_FLOAT_EQ(27.0f, robot.getNeighbors()[0]->getRadius());
}

// Test computeNewVelocity() in RCARobot.
TEST_F(RobotTest, ComputeNewVelocity)
{
  Robot robot;
  const float time_step = 1.0f;

  bool found_new_velocity = robot.computeNewVelocity(time_step);

  EXPECT_FALSE(found_new_velocity);
}
  
// Test constructors in VelocityObstacle.
TEST_F(ScalarTest, FSel)
{
  const float scalar1 = -1.0f;
  const float scalar2 = 0.0f;
  const float scalar3 = 1.0f;
  
  EXPECT_FLOAT_EQ(1.0f, FSel(scalar1, scalar2, scalar3));
  EXPECT_FLOAT_EQ(1.0f, FSel(scalar2, scalar3, scalar1));
  EXPECT_FLOAT_EQ(-1.0f, FSel(scalar3, scalar1, scalar2));
  EXPECT_FLOAT_EQ(0.0f, FSel(scalar3, scalar2, scalar1));
  EXPECT_FLOAT_EQ(-1.0f, FSel(scalar2, scalar1, scalar3));
  EXPECT_FLOAT_EQ(0.0f, FSel(scalar1, scalar3, scalar2));
}

// Test constructors in Vector2.
TEST_F(Vector2Test, Constructor)
{
  // Default constructor.
  const Vector2 vector1;

#ifdef HAS_SSE_EXTENSIONS
  float x1;
  float y1;
  
  _mm_store_ss(&x1, vector1.m128_);
  _mm_store_ss(&y1, _mm_shuffle_ps(vector1.m128_, vector1.m128_, _MM_SHUFFLE(1,1,1,1)));
  
  EXPECT_FLOAT_EQ(0.0f, x1);
  EXPECT_FLOAT_EQ(0.0f, y1);
#else
  EXPECT_FLOAT_EQ(0.0f, vector1.x_);
  EXPECT_FLOAT_EQ(0.0f, vector1.y_);
#endif

  // Constructor given x-coordinate and y-coordinate.
  const Vector2 vector2(1.0f, 2.0f);

#ifdef HAS_SSE_EXTENSIONS
  float x2;
  float y2;
  
  _mm_store_ss(&x2, vector2.m128_);
  _mm_store_ss(&y2, _mm_shuffle_ps(vector2.m128_, vector2.m128_, _MM_SHUFFLE(1,1,1,1)));
  
  EXPECT_FLOAT_EQ(1.0f, x2);
  EXPECT_FLOAT_EQ(2.0f, y2);
#else
  EXPECT_FLOAT_EQ(1.0f, vector2.x_);
  EXPECT_FLOAT_EQ(2.0f, vector2.y_);
#endif

  // Copy constructor.
  const Vector2 vector3(3.0f, 4.0f);
  const Vector2 vector4(vector3);

#ifdef HAS_SSE_EXTENSIONS
  float x4;
  float y4;
  
  _mm_store_ss(&x4, vector4.m128_);
  _mm_store_ss(&y4, _mm_shuffle_ps(vector4.m128_, vector4.m128_, _MM_SHUFFLE(1,1,1,1)));
  
  EXPECT_FLOAT_EQ(3.0f, x4);
  EXPECT_FLOAT_EQ(4.0f, y4);
#else
  EXPECT_FLOAT_EQ(3.0f, vector4.x_);
  EXPECT_FLOAT_EQ(4.0f, vector4.y_);
#endif
}

// Test getters in Vector2.
TEST_F(Vector2Test, Get)
{
  const Vector2 vector(2.0f, 4.0f);

  const float x = vector.getX();
  const float y = vector.getY();

  EXPECT_FLOAT_EQ(2.0f, x);
  EXPECT_FLOAT_EQ(4.0f, y);
}

// Test setters in Vector2.
TEST_F(Vector2Test, Set)
{
  Vector2 vector;

  vector.setX(3.0f);
  vector.setY(6.0f);

  EXPECT_FLOAT_EQ(3.0f, vector.getX());
  EXPECT_FLOAT_EQ(6.0f, vector.getY());
}

// Test setAbsolute() and absolute() in Vector2 and Absolute().
TEST_F(Vector2Test, Absolute)
{
  Vector2 vector1(-1.0f, 2.0f);
  const Vector2 vector2(3.0f, -4.0f);

  // Test Vector2::setAbsolute().
  vector1.setAbsolute();
  
  EXPECT_FLOAT_EQ(1.0f, vector1.getX());
  EXPECT_FLOAT_EQ(2.0f, vector1.getY());
  
  // Test Vector2::absolute().
  const Vector2 result1 = vector2.absolute();
  
  EXPECT_FLOAT_EQ(3.0f, result1.getX());
  EXPECT_FLOAT_EQ(4.0f, result1.getY());
  
  // Test Absolute().
  const Vector2 result2 = Absolute(vector2);
  
  EXPECT_FLOAT_EQ(3.0f, result2.getX());
  EXPECT_FLOAT_EQ(4.0f, result2.getY());
}
  
// Test angle() in Vector2 and Angle().
TEST_F(Vector2Test, Angle)
{
  const Vector2 vector1(0.0f, 1.0f);
  const Vector2 vector2(1.0f, 0.0f);

  // Test Vector2::angle().
  EXPECT_FLOAT_EQ(NAV_RECIP_PI / 2.0f, vector1.angle(vector2));
  EXPECT_FLOAT_EQ(NAV_RECIP_PI / 2.0f, vector2.angle(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector1.angle(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector2.angle(vector2));

  // Test Angle().
  EXPECT_FLOAT_EQ(NAV_RECIP_PI / 2.0f, Angle(vector1, vector2));
  EXPECT_FLOAT_EQ(NAV_RECIP_PI / 2.0f, Angle(vector2, vector1));
  EXPECT_FLOAT_EQ(0.0f, Angle(vector1, vector1));
  EXPECT_FLOAT_EQ(0.0f, Angle(vector2, vector2));
}

// Test cross() in Vector2 and Cross().
TEST_F(Vector2Test, Cross)
{
  const Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(3.0f, 4.0f);

  // Test Vector2::cross()
  EXPECT_FLOAT_EQ(-2.0f, vector1.cross(vector2));
  EXPECT_FLOAT_EQ(2.0f, vector2.cross(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector1.cross(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector2.cross(vector2));

  // Test Cross()
  EXPECT_FLOAT_EQ(-2.0f, Cross(vector1, vector2));
  EXPECT_FLOAT_EQ(2.0f, Cross(vector2, vector1));
  EXPECT_FLOAT_EQ(0.0f, Cross(vector1, vector1));
  EXPECT_FLOAT_EQ(0.0f, Cross(vector2, vector2));
}

// Test distance2() and distance() in Vector2 and Distance2() and Distance().
TEST_F(Vector2Test, Distance)
{
  const Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(4.0f, 6.0f);

  // Test Vector2::distance2().
  EXPECT_FLOAT_EQ(25.0f, vector1.distance2(vector2));
  EXPECT_FLOAT_EQ(25.0f, vector2.distance2(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector1.distance2(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector2.distance2(vector2));

  // Test Vector2::distance().
  EXPECT_FLOAT_EQ(5.0f, vector1.distance(vector2));
  EXPECT_FLOAT_EQ(5.0f, vector2.distance(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector1.distance(vector1));
  EXPECT_FLOAT_EQ(0.0f, vector2.distance(vector2));

  // Test Distance2().
  EXPECT_FLOAT_EQ(25.0f, Distance2(vector1, vector2));
  EXPECT_FLOAT_EQ(25.0f, Distance2(vector2, vector1));
  EXPECT_FLOAT_EQ(0.0f, Distance2(vector1, vector1));
  EXPECT_FLOAT_EQ(0.0f, Distance2(vector2, vector2));

  // Test Distance().
  EXPECT_FLOAT_EQ(5.0f, Distance(vector1, vector2));
  EXPECT_FLOAT_EQ(5.0f, Distance(vector2, vector1));
  EXPECT_FLOAT_EQ(0.0f, Distance(vector1, vector1));
  EXPECT_FLOAT_EQ(0.0f, Distance(vector2, vector2));
}

// Test dot() in Vector2 and Dot().
TEST_F(Vector2Test, Dot)
{
  const Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(3.0f, 4.0f);

  // Test Vector2::dot().
  EXPECT_FLOAT_EQ(11.0f, vector1.dot(vector2));
  EXPECT_FLOAT_EQ(11.0f, vector2.dot(vector1));
  EXPECT_FLOAT_EQ(5.0f, vector1.dot(vector1));
  EXPECT_FLOAT_EQ(25.0f, vector2.dot(vector2));

  // Test Dot().
  EXPECT_FLOAT_EQ(11.0f, Dot(vector1, vector2));
  EXPECT_FLOAT_EQ(11.0f, Dot(vector2, vector1));
  EXPECT_FLOAT_EQ(5.0f, Dot(vector1, vector1));
  EXPECT_FLOAT_EQ(25.0f, Dot(vector2, vector2));
}

// Test length2() and length() in Vector2 and Length2() and Length().
TEST_F(Vector2Test, Length)
{
  const Vector2 vector(3.0f, 4.0f);

  // Test Vector2::length2().
  EXPECT_FLOAT_EQ(25.0f, vector.length2());

  // Test Vector2::length().
  EXPECT_FLOAT_EQ(5.0f, vector.length());

  // Test Length2().
  EXPECT_FLOAT_EQ(25.0f, Length2(vector));

  // Test Length().
  EXPECT_FLOAT_EQ(5.0f, Length(vector));
}
  
// Test max() in Vector2 and Max().
TEST_F(Vector2Test, Max)
{
  const Vector2 vector1(-1.0f, 2.0f);
  const Vector2 vector2(-3.0f, 4.0f);
  
  // Test Vector2::max().
  const Vector2 result1 = vector1.max(vector2);
  const Vector2 result2 = vector2.max(vector1);
  const Vector2 result3 = vector1.max(vector1);
  const Vector2 result4 = vector2.max(vector2);
  
  EXPECT_FLOAT_EQ(-1.0f, result1.getX());
  EXPECT_FLOAT_EQ(4.0f, result1.getY());
  EXPECT_FLOAT_EQ(-1.0f, result2.getX());
  EXPECT_FLOAT_EQ(4.0f, result2.getY());
  EXPECT_FLOAT_EQ(-1.0f, result3.getX());
  EXPECT_FLOAT_EQ(2.0f, result3.getY());
  EXPECT_FLOAT_EQ(-3.0f, result4.getX());
  EXPECT_FLOAT_EQ(4.0f, result4.getY());
  
  // Test Max().
  const Vector2 result5 = Max(vector1, vector2);
  const Vector2 result6 = Max(vector2, vector1);
  const Vector2 result7 = Max(vector1, vector1);
  const Vector2 result8 = Max(vector2, vector2);
  
  EXPECT_FLOAT_EQ(-1.0f, result5.getX());
  EXPECT_FLOAT_EQ(4.0f, result5.getY());
  EXPECT_FLOAT_EQ(-1.0f, result6.getX());
  EXPECT_FLOAT_EQ(4.0f, result6.getY());
  EXPECT_FLOAT_EQ(-1.0f, result7.getX());
  EXPECT_FLOAT_EQ(2.0f, result7.getY());
  EXPECT_FLOAT_EQ(-3.0f, result8.getX());
  EXPECT_FLOAT_EQ(4.0f, result8.getY());
}
  
// Test min() in Vector2 and Min().
TEST_F(Vector2Test, Min)
{
  const Vector2 vector1(-1.0f, 2.0f);
  const Vector2 vector2(-3.0f, 4.0f);
  
  // Test Vector2::min().
  const Vector2 result1 = vector1.min(vector2);
  const Vector2 result2 = vector2.min(vector1);
  const Vector2 result3 = vector1.min(vector1);
  const Vector2 result4 = vector2.min(vector2);
  
  EXPECT_FLOAT_EQ(-3.0f, result1.getX());
  EXPECT_FLOAT_EQ(2.0f, result1.getY());
  EXPECT_FLOAT_EQ(-3.0f, result2.getX());
  EXPECT_FLOAT_EQ(2.0f, result2.getY());
  EXPECT_FLOAT_EQ(-1.0f, result3.getX());
  EXPECT_FLOAT_EQ(2.0f, result3.getY());
  EXPECT_FLOAT_EQ(-3.0f, result4.getX());
  EXPECT_FLOAT_EQ(4.0f, result4.getY());
  
  // Test Min().
  const Vector2 result5 = Min(vector1, vector2);
  const Vector2 result6 = Min(vector2, vector1);
  const Vector2 result7 = Min(vector1, vector1);
  const Vector2 result8 = Min(vector2, vector2);
  
  EXPECT_FLOAT_EQ(-3.0f, result5.getX());
  EXPECT_FLOAT_EQ(2.0f, result5.getY());
  EXPECT_FLOAT_EQ(-3.0f, result6.getX());
  EXPECT_FLOAT_EQ(2.0f, result6.getY());
  EXPECT_FLOAT_EQ(-1.0f, result7.getX());
  EXPECT_FLOAT_EQ(2.0f, result7.getY());
  EXPECT_FLOAT_EQ(-3.0f, result8.getX());
  EXPECT_FLOAT_EQ(4.0f, result8.getY());
}

// Test normalize() and normalized() in Vector2 and Normalized().
TEST_F(Vector2Test, Normalize)
{
  Vector2 vector1(3.0f, 4.0f);
  const Vector2 vector2(4.0f, 3.0f);

  // Test Vector2::normalize().
  vector1.normalize();

#ifdef HAS_SSE_EXTENSIONS
  EXPECT_NEAR(0.6f, vector1.getX(), 1.0e-4f);
  EXPECT_NEAR(0.8f, vector1.getY(), 1.0e-4f);
#else
  EXPECT_FLOAT_EQ(0.6f, vector1.getX());
  EXPECT_FLOAT_EQ(0.8f, vector1.getY());
#endif

  // Test Vector2::normalized().
  const Vector2 result1 = vector2.normalized();

#ifdef HAS_SSE_EXTENSIONS
  EXPECT_NEAR(0.8f, result1.getX(), 1.0e-4f);
  EXPECT_NEAR(0.6f, result1.getY(), 1.0e-4f);  
#else
  EXPECT_FLOAT_EQ(0.8f, result1.getX());
  EXPECT_FLOAT_EQ(0.6f, result1.getY());
#endif

  // Test Normalized().
  const Vector2 result2 = Normalized(vector2);

#ifdef HAS_SSE_EXTENSIONS
  EXPECT_NEAR(0.8f, result2.getX(), 1.0e-4f);
  EXPECT_NEAR(0.6f, result2.getY(), 1.0e-4f);
#else
  EXPECT_FLOAT_EQ(0.8f, result2.getX());
  EXPECT_FLOAT_EQ(0.6f, result2.getY());
#endif
}

// Test orientation() in Vector2 and Orientation().
TEST_F(Vector2Test, Orientation)
{
  const Vector2 vector(1.0f, -1.0f);

  // Test Vector2::orientation().
  EXPECT_FLOAT_EQ(-NAV_RECIP_PI / 4.0f, vector.orientation());

  // Test Orientation().
  EXPECT_FLOAT_EQ(-NAV_RECIP_PI / 4.0f, Orientation(vector));
}

// Test setOrthogonal() and orthogonal() in Vector2 and Orthogonal().
TEST_F(Vector2Test, Orthogonal)
{
  Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(3.0f, 4.0f);
  
  // Test Vector2::setOrthogonal().
  vector1.setOrthogonal();
  
  EXPECT_FLOAT_EQ(-2.0f, vector1.getX());
  EXPECT_FLOAT_EQ(1.0f, vector1.getY());
  
  // Test Vector2::orthogonal().
  const Vector2 result1 = vector2.orthogonal();

  EXPECT_FLOAT_EQ(-4.0f, result1.getX());
  EXPECT_FLOAT_EQ(3.0f, result1.getY());

  // Test Orthogonal().
  const Vector2 result2 = Orthogonal(vector2);

  EXPECT_FLOAT_EQ(-4.0f, result2.getX());
  EXPECT_FLOAT_EQ(3.0f, result2.getY());
}
  
// Test Select().
TEST_F(Vector2Test, Select)
{
  const Vector2 vector1(-1.0f, 2.0f);
  const Vector2 vector2(3.0f, -4.0f);
  const Vector2 vector3(-5.0f, 6.0f);
  
  const Vector2 result1 = Select(vector1, vector2, vector3);
  const Vector2 result2 = Select(vector2, vector3, vector1);
  const Vector2 result3 = Select(vector3, vector1, vector2);
  
  EXPECT_FLOAT_EQ(-5.0f, result1.getX());
  EXPECT_FLOAT_EQ(-4.0f, result1.getY());
  EXPECT_FLOAT_EQ(-5.0f, result2.getX());
  EXPECT_FLOAT_EQ(2.0f, result2.getY());
  EXPECT_FLOAT_EQ(3.0f, result3.getX());
  EXPECT_FLOAT_EQ(2.0f, result3.getY());
}

// Test operator==() in Vector2.
TEST_F(Vector2Test, Equal)
{
  const Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(1.0f, 2.0f);
  const Vector2 vector3(2.0f, 1.0f);

  EXPECT_TRUE(vector1 == vector2);
  EXPECT_TRUE(vector2 == vector1);
  EXPECT_TRUE(vector1 == vector1);
  EXPECT_TRUE(vector2 == vector2);
  EXPECT_TRUE(vector3 == vector3);
  EXPECT_FALSE(vector1 == vector3);
  EXPECT_FALSE(vector3 == vector1);
  EXPECT_FALSE(vector2 == vector3);
  EXPECT_FALSE(vector3 == vector2);
}

// Test operator!=() in Vector2.
TEST_F(Vector2Test, NotEqual)
{
  const Vector2 vector1(3.0f, 4.0f);
  const Vector2 vector2(3.0f, 4.0f);
  const Vector2 vector3(4.0f, 3.0f);

  EXPECT_TRUE(vector1 != vector3);
  EXPECT_TRUE(vector3 != vector1);
  EXPECT_TRUE(vector2 != vector3);
  EXPECT_TRUE(vector3 != vector2);
  EXPECT_FALSE(vector1 != vector2);
  EXPECT_FALSE(vector2 != vector1);
  EXPECT_FALSE(vector1 != vector1);
  EXPECT_FALSE(vector2 != vector2);
  EXPECT_FALSE(vector3 != vector3);
}

// Test operator-() in Vector2.
TEST_F(Vector2Test, OperatorNegate)
{
  const Vector2 vector(1.0f, 2.0f);

  const Vector2 result = -vector;

  EXPECT_FLOAT_EQ(-1.0f, result.getX());
  EXPECT_FLOAT_EQ(-2.0f, result.getY());
}

// Test operator*=() in Vector2.
TEST_F(Vector2Test, OperatorCompoundMultiply)
{
  Vector2 vector(2.0f, 4.0f);
  const float scalar = 6.0f;

  vector *= scalar;

  EXPECT_FLOAT_EQ(12.0f, vector.getX());
  EXPECT_FLOAT_EQ(24.0f, vector.getY());
}

// Test operator/=() in Vector2.
TEST_F(Vector2Test, OperatorCompoundDivide)
{
  Vector2 vector(3.0f, 6.0f);
  const float scalar = 3.0f;

  vector /= scalar;

  EXPECT_FLOAT_EQ(1.0f, vector.getX());
  EXPECT_FLOAT_EQ(2.0f, vector.getY());
}

// Test operator+=() in Vector2.
TEST_F(Vector2Test, OperatorCompoundAdd)
{
  Vector2 vector1(2.0f, 4.0f);
  const Vector2 vector2(6.0f, 8.0f);

  vector1 += vector2;

  EXPECT_FLOAT_EQ(8.0f, vector1.getX());
  EXPECT_FLOAT_EQ(12.0f, vector1.getY());
}

// Test operator-=() in Vector2.
TEST_F(Vector2Test, OperatorCompoundSubtract)
{
  Vector2 vector1(2.0f, 4.0f);
  const Vector2 vector2(6.0f, 8.0f);

  vector1 -= vector2;

  EXPECT_FLOAT_EQ(-4.0f, vector1.getX());
  EXPECT_FLOAT_EQ(-4.0f, vector1.getY());
}

// Test operator*() in Vector2.
TEST_F(Vector2Test, OperatorMultiply)
{
  const Vector2 vector(1.0f, 2.0f);
  const float scalar = 3.0f;

  const Vector2 result1 = vector * scalar;

  EXPECT_FLOAT_EQ(3.0f, result1.getX());
  EXPECT_FLOAT_EQ(6.0f, result1.getY());

  const Vector2 result2 = scalar * vector;

  EXPECT_FLOAT_EQ(3.0f, result2.getX());
  EXPECT_FLOAT_EQ(6.0f, result2.getY());
}

// Test operator/() in Vector2.
TEST_F(Vector2Test, OperatorDivide)
{
  const Vector2 vector(2.0f, 4.0f);
  const float scalar = 2.0f;

  const Vector2 result = vector / scalar;

  EXPECT_FLOAT_EQ(1.0f, result.getX());
  EXPECT_FLOAT_EQ(2.0f, result.getY());
}

// Test operator+() in Vector2.
TEST_F(Vector2Test, OperatorAdd)
{
  const Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(3.0f, 4.0f);

  const Vector2 result1 = vector1 + vector2;

  EXPECT_FLOAT_EQ(4.0f, result1.getX());
  EXPECT_FLOAT_EQ(6.0f, result1.getY());

  const Vector2 result2 = vector2 + vector1;

  EXPECT_FLOAT_EQ(4.0f, result2.getX());
  EXPECT_FLOAT_EQ(6.0f, result2.getY());

  const Vector2 result3 = vector1 + vector1;

  EXPECT_FLOAT_EQ(2.0f, result3.getX());
  EXPECT_FLOAT_EQ(4.0f, result3.getY());

  const Vector2 result4 = vector2 + vector2;

  EXPECT_FLOAT_EQ(6.0f, result4.getX());
  EXPECT_FLOAT_EQ(8.0f, result4.getY());
}

// Test operator-() in Vector2.
TEST_F(Vector2Test, OperatorSubtract)
{
  const Vector2 vector1(1.0f, 2.0f);
  const Vector2 vector2(3.0f, 4.0f);

  const Vector2 result1 = vector1 - vector2;

  EXPECT_FLOAT_EQ(-2.0f, result1.getX());
  EXPECT_FLOAT_EQ(-2.0f, result1.getY());

  const Vector2 result2 = vector2 - vector1;

  EXPECT_FLOAT_EQ(2.0f, result2.getX());
  EXPECT_FLOAT_EQ(2.0f, result2.getY());

  const Vector2 result3 = vector1 - vector1;

  EXPECT_FLOAT_EQ(0.0f, result3.getX());
  EXPECT_FLOAT_EQ(0.0f, result3.getY());

  const Vector2 result4 = vector2 - vector2;

  EXPECT_FLOAT_EQ(0.0f, result4.getX());
  EXPECT_FLOAT_EQ(0.0f, result4.getY());
}
}

// Run all unit tests for this package.
int main(int argc, char* argv[])
{
  testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}
